#include "STP23.h"
#include <math.h>
extern uint16_t distance,distance_y;
extern CAN_HandleTypeDef hcan1;
void STP23_Init(STP23_PID_Controller* ctrl, 
               float kp, float ki, float kd,
               float error_threshold,
							float outputMin	,
               float outputMax) {
    ctrl->kp = kp;
    ctrl->ki = ki;
    ctrl->kd = kd;
    ctrl->errorThreshold = error_threshold;  // 初始化阈值
    ctrl->outputMax = outputMax;
    ctrl->outputMin = outputMin;
    ctrl->errorIntegral = 0;
    ctrl->lastError = 0;
    ctrl->output = 0;
}

void STP23_PID_Calculate(STP23_PID_Controller* ctrl) {
    float error = ctrl->targetDistance - ctrl->currentDistance;
    
    // 死区处理
    if(fabsf(error) < ctrl->errorThreshold) {
        ctrl->output = 0;
        ctrl->errorIntegral = 0;  // 重置积分项
        ctrl->lastError = 0;
        return;
    }
    
    // 积分项累计（带抗饱和）
    if(ctrl->errorIntegral < (ctrl->outputMax / 2) && ctrl->errorIntegral > -(ctrl->outputMax / 2)) {
        ctrl->errorIntegral += error;
    }
//    ctrl->errorIntegral=ctrl->lastError+error;
    // 微分项计算
    float derivative = error - ctrl->lastError;
    
    // PID计算
    ctrl->output = (ctrl->kp * error) + 
                  (ctrl->ki * ctrl->errorIntegral) + 
                  (ctrl->kd * derivative);
    
    // 输出限幅
		if(ctrl->output > ctrl->outputMax) ctrl->output=ctrl->outputMax;
    if(ctrl->output < -(ctrl->outputMax)) ctrl->output= -ctrl->outputMax;
    
    // 保存误差记录
    ctrl->lastError = error;
}

int16_t STP23_Servo(STP23_PID_Controller* ctrl, float target_mm, float current_mm) {
    ctrl->targetDistance = target_mm;
    ctrl->currentDistance = current_mm;
    STP23_PID_Calculate(ctrl);
    return (int16_t)(ctrl->output * -1); 
}

void STP23_XAxisPIDCalculate(STP23_PID_Controller* ctrl) {
    float error = ctrl->targetDistance - ctrl->currentDistance;
    
    // 死区处理
    float absError = error;
    if (absError < 0) {
        absError = -absError;
    }
    if (absError < ctrl->errorThreshold) {
        ctrl->output = 0;
        ctrl->errorIntegral = 0;  // 重置积分项
        ctrl->lastError = 0;
        return;
    }
    
    // 积分项累计（带抗饱和）
    if (ctrl->errorIntegral < (ctrl->outputMax / 2) && ctrl->errorIntegral > -(ctrl->outputMax / 2)) {
        ctrl->errorIntegral += error;
    }
    
    // 微分项计算
    float derivative = error - ctrl->lastError;
    
    // PID计算
    ctrl->output = (ctrl->kp * error) + 
                  (ctrl->ki * ctrl->errorIntegral) + 
                  (ctrl->kd * derivative);
    
    // 输出限幅到 -6000 到 -3000 或者 3000 到 6000
    if (ctrl->output > 0) {
        if (ctrl->output < ctrl->outputMin) {
            ctrl->output = ctrl->outputMin;
        }
        if (ctrl->output > ctrl->outputMax) {
            ctrl->output = ctrl->outputMax;
        }
    } else {
        if (ctrl->output > -ctrl->outputMin) {
            ctrl->output = -ctrl->outputMin;
        }
        if (ctrl->output < -ctrl->outputMax) {
            ctrl->output = -ctrl->outputMax;
        }
    }
    
    // 保存误差记录
    ctrl->lastError = error;
}

// 新增X轴伺服函数
int16_t STP23_XAxisServo(STP23_PID_Controller* ctrl, float target_mm, float current_mm) {
    ctrl->targetDistance = target_mm;
    ctrl->currentDistance = current_mm;
    STP23_XAxisPIDCalculate(ctrl);
    return (int16_t)(-ctrl->output); 
}
void M2006_Y_Control_Outwhile(STP23_PID_Controller* ctrl,uint16_t delay_us,uint16_t target_mm)
{
	uint8_t flag=0;
	ctrl->targetDistance = target_mm;
	while(!flag)
	{
				ctrl->currentDistance = distance_y;
		    float error = ctrl->targetDistance - ctrl->currentDistance;
		    float absError = error;
				if (absError < 0) {
						absError = -absError;
				}

				if (ctrl->errorIntegral < (ctrl->outputMax / 2) && ctrl->errorIntegral > -(ctrl->outputMax / 2)) {
        ctrl->errorIntegral += error;
				}
    
				// 微分项计算
				float derivative = error - ctrl->lastError;
				
				// PID计算
				ctrl->output = (ctrl->kp * error) + 
											(ctrl->ki * ctrl->errorIntegral) + 
											(ctrl->kd * derivative);
				if (ctrl->output > 0) {
						if (ctrl->output < ctrl->outputMin) {
								ctrl->output = ctrl->outputMin;
						}
						if (ctrl->output > ctrl->outputMax) {
								ctrl->output = ctrl->outputMax;
						}
				}
				else {
						if (ctrl->output > -ctrl->outputMin) {
								ctrl->output = -ctrl->outputMin;
						}
						if (ctrl->output < -ctrl->outputMax) {
								ctrl->output = -ctrl->outputMax;
						}
				}
				if (absError < ctrl->errorThreshold) {
						ctrl->output = 0;
						ctrl->errorIntegral = 0;  // 重置积分项
						ctrl->lastError = 0;
						flag=1;
				}				
		CanTransmit_DJI_1234(&hcan1, 0, ctrl->output, -ctrl->output, 0);
//		HAL_GPIO_WritePin(DIRp57_GPIO_Port,DIRp57_Pin,1);
//		HAL_GPIO_WritePin(PULp57_GPIO_Port,PULp57_Pin,GPIO_PIN_SET);
//		HAL_DelayUs(delay_us);
//		HAL_GPIO_WritePin(PULp57_GPIO_Port,PULp57_Pin,GPIO_PIN_RESET);
//		HAL_DelayUs(delay_us);
//		HAL_GPIO_WritePin(DIRp57_GPIO_Port,DIRp57_Pin,0);	
//		HAL_GPIO_WritePin(PULp57_GPIO_Port,PULp57_Pin,GPIO_PIN_SET);
//		HAL_DelayUs(delay_us);
//		HAL_GPIO_WritePin(PULp57_GPIO_Port,PULp57_Pin,GPIO_PIN_RESET);
//		HAL_DelayUs(delay_us);
	}
}	
void M2006_Y_Control_Outwhile_Test1(STP23_PID_Controller* ctrl,uint16_t delay_us,uint16_t target_mm)
{
	uint8_t flag=0;
	ctrl->targetDistance = target_mm;
	while(!flag)
	{
				ctrl->currentDistance = distance_y;
		    float error = ctrl->targetDistance - ctrl->currentDistance;
		    float absError = error;
				if (absError < 0) {
						absError = -absError;
				}

				if (ctrl->errorIntegral < (ctrl->outputMax / 2) && ctrl->errorIntegral > -(ctrl->outputMax / 2)) {
        ctrl->errorIntegral += error;
				}
    
				// 微分项计算
				float derivative = error - ctrl->lastError;
				
				// PID计算
				ctrl->output = (ctrl->kp * error) + 
											(ctrl->ki * ctrl->errorIntegral) + 
											(ctrl->kd * derivative);
				if (ctrl->output > 0) {
						if (ctrl->output < ctrl->outputMin) {
								ctrl->output = ctrl->outputMin;
						}
						if (ctrl->output > ctrl->outputMax) {
								ctrl->output = ctrl->outputMax;
						}
				}
				else {
						if (ctrl->output > -ctrl->outputMin) {
								ctrl->output = -ctrl->outputMin;
						}
						if (ctrl->output < -ctrl->outputMax) {
								ctrl->output = -ctrl->outputMax;
						}
				}
				if (absError < ctrl->errorThreshold) {
						ctrl->output = 0;
						ctrl->errorIntegral = 0;  // 重置积分项
						ctrl->lastError = 0;
						flag=1;
				}				
		CanTransmit_DJI_1234(&hcan1, 0, ctrl->output, -ctrl->output, 0);
	}
}	
void M2006xy_Oled_Outwhile(float target_x_mm,float target_y_mm,STP23_PID_Controller* stp23_ctrl,STP23_PID_Controller* stp23_ctrl_y)
{
	int16_t output=0;
	int16_t output_y=0;
	uint8_t flag=0;
	char	distance_str[10]="";
	char	distance_y_str[10]="";
	while(!flag)
	{
		OLED_Clear();
		output=STP23_XAxisServo(stp23_ctrl, target_x_mm, distance);
		output_y=-STP23_XAxisServo(stp23_ctrl_y,target_y_mm, distance_y);
		CanTransmit_DJI_1234(&hcan1, output, output_y, -output_y, 0);
		OLED_ShowString(1,1,"distance",OLED_8X16);
		floatToString(distance,distance_str,0);
		floatToString(distance_y,distance_y_str,0);
		OLED_ShowString(1,1+16,distance_str,OLED_8X16);
		OLED_ShowString(1,1+16+16,distance_y_str,OLED_8X16);	
		OLED_Update();
		if(
			((stp23_ctrl->currentDistance	>	(stp23_ctrl->targetDistance	-	stp23_ctrl->errorThreshold))
			&&(stp23_ctrl->currentDistance	<	(stp23_ctrl->targetDistance	+	stp23_ctrl->errorThreshold)))
			&&((stp23_ctrl_y->currentDistance	>	(stp23_ctrl_y->targetDistance	-	stp23_ctrl_y->errorThreshold)) 
			&&(stp23_ctrl_y->currentDistance	<	(stp23_ctrl_y->targetDistance	+	stp23_ctrl_y->errorThreshold))) 		
		)
		flag=1;
	}
}
void M2006xy_delayus_Outwhile(uint32_t delay_us,float target_x_mm,float target_y_mm,STP23_PID_Controller* stp23_ctrl,STP23_PID_Controller* stp23_ctrl_y)
{
	int16_t output=0;
	int16_t output_y=0;
	uint8_t flag=0;
	char	distance_str[10]="";
	char	distance_y_str[10]="";
	while(!flag)
	{
		output=STP23_XAxisServo(stp23_ctrl, target_x_mm, distance);
		output_y=-STP23_XAxisServo(stp23_ctrl_y,target_y_mm, distance_y);
		CanTransmit_DJI_1234(&hcan1, output, output_y, -output_y, 0);
		if((stp23_ctrl->lastError==0)&&(stp23_ctrl_y->lastError==0)) flag=1;
		HAL_DelayUs(delay_us);
	}
}
void M2006RunInWhile(STP23_PID_Controller* stp23_ctrl,STP23_PID_Controller* stp23_ctrl_y,float target_x_mm,float target_y_mm)
{
		int16_t output_m2006,output_m2006_y;
		output_m2006=STP23_XAxisServo(stp23_ctrl, target_x_mm, distance);
		output_m2006_y=-STP23_XAxisServo(stp23_ctrl_y, target_y_mm, distance_y);	
		CanTransmit_DJI_1234(&hcan1, output_m2006, output_m2006_y, -output_m2006_y, 0);		
}
void M2006XPositionServo(float ref, DJI_t * motor)
{
	motor->posPID.ref=ref;
	motor->posPID.cur_error=ref-distance;
	if(motor->posPID.cur_error<10&&motor->posPID.cur_error>-10)
	{
		motor->speedPID.output=0;
	}
	else
	{
		if(motor->posPID.cur_error<100&&motor->posPID.cur_error>-100)
		{
			if(motor->posPID.cur_error>0) speedServo(-1500,motor);
			else speedServo(1500,motor);
		}	
		else 
		{
			if(motor->posPID.cur_error>0) speedServo(-3800,motor);
			else speedServo(3800,motor);
		}
	}
}
// void M2006YPositionServo(float ref, DJI_t * motor)
// {
// 	motor->posPID.ref=ref;
// 	motor->posPID.cur_error=ref-distance_y;
// 	if(motor->posPID.cur_error<10&&motor->posPID.cur_error>-10)
// 	{
// 		motor->speedPID.output=0;
// 	}
// 	else
// 	{
// 		if(motor->posPID.cur_error<150&&motor->posPID.cur_error>-150)
// 		{
// 			if(motor->posPID.cur_error>0) speedServo(750,motor);
// 			else speedServo(-750,motor);
// 		}
// 		else
// 		{
// 			if(motor->posPID.cur_error>0) speedServo(7000,motor);
// 			else speedServo(-7000,motor);
// 		}
// 	}
// }
void M2006YPositionServo(float ref, DJI_t * motor)
{

	motor->posPID.ref=ref;
	motor->posPID.cur_error=ref-distance_y;
	if(motor->posPID.cur_error<10&&motor->posPID.cur_error>-10)
	{
		motor->speedPID.ref=0;
		motor->speedPID.fdb=0;
		motor->speedPID.cur_error=0;
		motor->speedPID.error[0]=0;
		motor->speedPID.error[1]=0;
		motor->posPID.cur_error=0;
		motor->posPID.error[0]=0;
		motor->posPID.error[1]=0;
		motor->speedPID.output=0;
	}
	else
	{
		if(motor->posPID.cur_error<150&&motor->posPID.cur_error>-150)
		{
			if(motor->posPID.cur_error>0) speedServo(750,motor);
			else speedServo(-750,motor);
		}
		else
		{
			motor->speedPID.ref+= motor->posPID.cur_error;
			if(motor->speedPID.ref>5000) motor->speedPID.ref=5000;
			if(motor->speedPID.ref<-5000) motor->speedPID.ref=-5000;
			speedServo(motor->speedPID.ref,motor);
		}
	}
	motor->posPID.error[0]=motor->posPID.cur_error;
	motor->posPID.error[1]=motor->posPID.error[0];
}

/* 腾讯元宝写的双环pid*/
void M2006Y_PosVelServo(float target_mm, DJI_t *motor_y) {
    static float last_error = 0;
    static float integral = 0;
    
    // 位置环计算
    motor_y->posPID.ref = target_mm;
    motor_y->posPID.cur_error = target_mm - distance_y;
    
    // 积分分离防止饱和
    if (fabs(motor_y->posPID.cur_error) < 50) {
        integral += motor_y->posPID.cur_error;
    } else {
        integral = 0;
    }
    
    // 位置PID计算（输出为速度设定值）
    float vel_setpoint = 
        motor_y->posPID.KP * motor_y->posPID.cur_error +
        motor_y->posPID.KI * integral +
        motor_y->posPID.KD * (motor_y->posPID.cur_error - last_error);
        
    last_error = motor_y->posPID.cur_error;
    
    // 速度环计算
    motor_y->speedPID.ref = vel_setpoint;
    motor_y->speedPID.cur_error = vel_setpoint - motor_y->AxisData.AxisVelocity;
    
    // 速度PID计算
    float pwm_output = 
        motor_y->speedPID.KP * motor_y->speedPID.cur_error +
        motor_y->speedPID.KI * motor_y->speedPID.error[0] +
        motor_y->speedPID.KD * (motor_y->speedPID.cur_error - motor_y->speedPID.error[1]);
        
    // 更新误差历史
    motor_y->speedPID.error[1] = motor_y->speedPID.error[0];
    motor_y->speedPID.error[0] = motor_y->speedPID.cur_error;
    
    // 限幅处理
    if (pwm_output > motor_y->speedPID.outputMax) {
        pwm_output = motor_y->speedPID.outputMax;
    } else if (pwm_output < -motor_y->speedPID.outputMax) {
        pwm_output = -motor_y->speedPID.outputMax;
    }
    
    // 输出控制
    motor_y->speedPID.output = pwm_output;
}
/*基于位置闭环改造*/
void YpositionServo_lidar(float ref, DJI_t *motor)
{
	motor->posPID.ref = ref;
	motor->posPID.fdb = distance_y;
	PID_Calc(&motor->posPID);

	motor->speedPID.ref = motor->posPID.output;
	motor->speedPID.fdb = motor->FdbData.rpm;
	PID_Calc(&(motor->speedPID));
	// if(motor->posPID.cur_error<150&&motor->posPID.cur_error>-150)
	// {
	// 		if(motor->posPID.cur_error>0) speedServo(100,motor);
	// 		else speedServo(-100,motor);
	// }
	if(motor->posPID.cur_error<200&&motor->posPID.cur_error>-200)
	{
//		motor->speedPID.output = motor->speedPID.output/3;
		if(motor->posPID.cur_error>0) motor->posPID.output=200;
		else motor->posPID.output=-200;
	}	
	if (motor->posPID.cur_error<10&&motor->posPID.cur_error>-10)
	{
		motor->speedPID.ref=0;
		motor->speedPID.fdb=0;
		motor->speedPID.cur_error=0;
		motor->speedPID.error[0]=0;
		motor->speedPID.error[1]=0;
		motor->posPID.cur_error=0;
		motor->posPID.error[0]=0;
		motor->posPID.error[1]=0;
		motor->speedPID.output=0;		
	}

	
}
void M2006XYPositionServo_WithStabilize(float ref_X, float ref_Y, DJI_t *motor_x, DJI_t *motor_y, CAN_HandleTypeDef *can_handle) {
    uint8_t flag_total = 0;
	while (!(flag_total)) {
        // 更新电机位置控制
        M2006XYPositionServo(ref_X, ref_Y, motor_x, motor_y);
        
        // 发送电机控制指令
        CanTransmit_DJI_1234(can_handle, 
                            motor_x->speedPID.output, 
                            motor_y->speedPID.output, 
                            -motor_y->speedPID.output, 
                            0);
        
        osDelay(1);  // 延时1ms
        
        // 检查是否达到目标位置并稳定
        if (motor_x->posPID.cur_error < 10 && motor_x->posPID.cur_error > -10 &&
            motor_y->posPID.cur_error < 10 && motor_y->posPID.cur_error > -10) {
            
            // 稳定后发送零指令停止电机
            CanTransmit_DJI_1234(can_handle, 0, 0, 0, 0);
            flag_total = 1;  // 设置完成标志
            
            // 延时1秒，保持稳定状态
            osDelay(1000);
        }
    }
}
void xyzControl(float ref_X, float ref_Y,float ref_z  ,DJI_t *motor_x, DJI_t *motor_y, CAN_HandleTypeDef *can_handle,Mg513_Pid *mg513pid)
{
    uint8_t flag_total = 0;
	while (!(flag_total)) {
        // 更新电机位置控制
        M2006XYPositionServo(ref_X, ref_Y, motor_x, motor_y);
        Mg513Run(mg513pid, ref_z);
        // 发送电机控制指令
        CanTransmit_DJI_1234(can_handle, 
                            motor_x->speedPID.output, 
                            motor_y->speedPID.output, 
                            -motor_y->speedPID.output, 
                            0);
        
        osDelay(1);  // 延时1ms
        
        // 检查是否达到目标位置并稳定
        if (motor_x->posPID.cur_error < 10 && motor_x->posPID.cur_error > -10 &&
            motor_y->posPID.cur_error < 10 && motor_y->posPID.cur_error > -10&&
			mg513pid->error[0] < 0.5 && mg513pid->error[0] > -0.5) {
			HAL_GPIO_WritePin(AIN1_3_GPIO_Port, AIN1_3_Pin, 1);
			HAL_GPIO_WritePin(AIN2_3_GPIO_Port, AIN2_3_Pin, 0); 	
			__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 300);            
            // 稳定后发送零指令停止电机
            CanTransmit_DJI_1234(can_handle, 0, 0, 0, 0);
            flag_total = 1;  // 设置完成标志
            
            // 延时1秒，保持稳定状态
            osDelay(500);
        }
    }	
}
void M2006XYPositionServo(float ref_X,float ref_y, DJI_t * motor_x,DJI_t * motor_y)
{
	M2006XPositionServo(ref_X,motor_x);
//	M2006YPositionServo(ref_y,motor_y);
	//暂时使用posvelservo
//	M2006Y_PosVelServo(ref_y,motor_y);
	YpositionServo_lidar(ref_y,motor_y);
}

/*基于位置闭环改造*/


void xyzControl_up(float ref_X, float ref_Y,float ref_z  ,DJI_t *motor_x, DJI_t *motor_y, CAN_HandleTypeDef *can_handle,Mg513_Pid *mg513pid)
{
    uint8_t flag_total = 0;
	while (!(flag_total)) {
        // 更新电机位置控制
        M2006XYPositionServo(ref_X, ref_Y, motor_x, motor_y);
        Mg513Run_up(mg513pid, ref_z);
        // 发送电机控制指令
        CanTransmit_DJI_1234(can_handle, 
                            motor_x->speedPID.output, 
                            motor_y->speedPID.output, 
                            -motor_y->speedPID.output, 
                            0);
        
        osDelay(1);  // 延时1ms
        
        // 检查是否达到目标位置并稳定
        if (motor_x->posPID.cur_error < 10 && motor_x->posPID.cur_error > -10 &&
            motor_y->posPID.cur_error < 10 && motor_y->posPID.cur_error > -10&&
			mg513pid->error[0] < 0.5 && mg513pid->error[0] > -0.5) {
			HAL_GPIO_WritePin(AIN1_3_GPIO_Port, AIN1_3_Pin, 1);
			HAL_GPIO_WritePin(AIN2_3_GPIO_Port, AIN2_3_Pin, 0); 	
			__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 300);            
            // 稳定后发送零指令停止电机
            CanTransmit_DJI_1234(can_handle, 0, 0, 0, 0);
            flag_total = 1;  // 设置完成标志
            
            // 延时1秒，保持稳定状态
            osDelay(500);
        }
    }	
}
void xyzControldelayoff(float ref_X, float ref_Y,float ref_z  ,DJI_t *motor_x, DJI_t *motor_y, CAN_HandleTypeDef *can_handle,Mg513_Pid *mg513pid)
{
    uint8_t flag_total = 0;
	while (!(flag_total)) {
        // 更新电机位置控制
        M2006XYPositionServo(ref_X, ref_Y, motor_x, motor_y);
        Mg513Run(mg513pid, ref_z);
        // 发送电机控制指令
        CanTransmit_DJI_1234(can_handle, 
                            motor_x->speedPID.output, 
                            motor_y->speedPID.output, 
                            -motor_y->speedPID.output, 
                            0);
        
        osDelay(1);  // 延时1ms
        
        // 检查是否达到目标位置并稳定
        if (motor_x->posPID.cur_error < 10 && motor_x->posPID.cur_error > -10 &&
            motor_y->posPID.cur_error < 10 && motor_y->posPID.cur_error > -10&&
			mg513pid->error[0] < 0.5 && mg513pid->error[0] > -0.5) {
			HAL_GPIO_WritePin(AIN1_3_GPIO_Port, AIN1_3_Pin, 1);
			HAL_GPIO_WritePin(AIN2_3_GPIO_Port, AIN2_3_Pin, 0); 	
			__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 300);            
            // 稳定后发送零指令停止电机
            CanTransmit_DJI_1234(can_handle, 0, 0, 0, 0);
            flag_total = 1;  // 设置完成标志
            
            // 延时1秒，保持稳定状态
            osDelay(1);
        }
    }	
}
void xyzControl_updelayoff(float ref_X, float ref_Y,float ref_z  ,DJI_t *motor_x, DJI_t *motor_y, CAN_HandleTypeDef *can_handle,Mg513_Pid *mg513pid)
{
    uint8_t flag_total = 0;
	while (!(flag_total)) {
        // 更新电机位置控制
        M2006XYPositionServo(ref_X, ref_Y, motor_x, motor_y);
        Mg513Run_up(mg513pid, ref_z);
        // 发送电机控制指令
        CanTransmit_DJI_1234(can_handle, 
                            motor_x->speedPID.output, 
                            motor_y->speedPID.output, 
                            -motor_y->speedPID.output, 
                            0);
        
        osDelay(1);  // 延时1ms
        
        // 检查是否达到目标位置并稳定
        if (motor_x->posPID.cur_error < 10 && motor_x->posPID.cur_error > -10 &&
            motor_y->posPID.cur_error < 10 && motor_y->posPID.cur_error > -10&&
			mg513pid->error[0] < 0.5 && mg513pid->error[0] > -0.5) {
			HAL_GPIO_WritePin(AIN1_3_GPIO_Port, AIN1_3_Pin, 1);
			HAL_GPIO_WritePin(AIN2_3_GPIO_Port, AIN2_3_Pin, 0); 	
			__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 300);            
            // 稳定后发送零指令停止电机
            CanTransmit_DJI_1234(can_handle, 0, 0, 0, 0);
            flag_total = 1;  // 设置完成标志
            
            // 延时1秒，保持稳定状态
            osDelay(1);
        }
    }	
}